﻿#include "Camera_DH.h"
#include "glog/logging.h"

Camera_DH::Camera_DH()
{
    m_bIsOpen[0] = false;
    m_bIsOpen[1] = false;

    m_pBmpInfo[0] = nullptr;
    m_pBmpInfo[1] = nullptr;

    m_pImgBuffer[0] = nullptr;
    m_pImgBuffer[1] = nullptr;

    m_accessStatus[0] = 0;
    m_accessStatus[1] = 0;

    // 初始化设备库
    GXInitLib();
}

Camera_DH::~Camera_DH()
{
    closeDevice();

    // 关闭设备库
    GXCloseLib();
}

int Camera_DH::prepareForImg(int index)
{
    bool bRet = true;

    //为GetImage参数分配空间
    m_stFrameData[index].pImgBuf = nullptr;
    m_stFrameData[index].pImgBuf = (BYTE*)malloc(sizeof(BYTE) * m_nPayLoadSize[index]);
    if (m_stFrameData[index].pImgBuf == nullptr)
    {
        bRet = false;
        LOG(INFO) << "error:" << m_nPayLoadSize[index] << std::endl;

        return bRet;
    }
    memset(m_stFrameData[index].pImgBuf, 0, sizeof(BYTE) * m_nPayLoadSize[index]);

    //---------------------------初始化bitmap头---------------------------
    m_pBmpInfo[index] = (BITMAPINFO*)m_chBmpBuf[index];
    m_pBmpInfo[index]->bmiHeader.biSize = sizeof(BITMAPINFOHEADER);
    m_pBmpInfo[index]->bmiHeader.biWidth = (LONG)m_nImageWidth[index];
    m_pBmpInfo[index]->bmiHeader.biHeight = (LONG)m_nImageHeight[index];

    m_pBmpInfo[index]->bmiHeader.biPlanes = 1;
    m_pBmpInfo[index]->bmiHeader.biCompression = BI_RGB;
    m_pBmpInfo[index]->bmiHeader.biSizeImage = 0;
    m_pBmpInfo[index]->bmiHeader.biXPelsPerMeter = 0;
    m_pBmpInfo[index]->bmiHeader.biYPelsPerMeter = 0;
    m_pBmpInfo[index]->bmiHeader.biClrUsed = 0;
    m_pBmpInfo[index]->bmiHeader.biClrImportant = 0;

    if (m_bIsColorFilter[index])
    {
        m_pBmpInfo[index]->bmiHeader.biBitCount = 24; // 彩色图像为24,黑白图像为8

        // 为经过RGB转换后的图像分配Buffer


        m_pImgBuffer[index] = nullptr;
        m_pImgBuffer[index] = (BYTE*)malloc(sizeof(BYTE) * m_nImageWidth[index] * m_nImageHeight[index] * 3);
        if (m_pImgBuffer[index] == nullptr)
        {
            bRet = false;
            LOG(INFO) << "error:" << m_nImageWidth[index] * m_nImageHeight[index] * 3 << std::endl;

            return bRet;
        }
        memset(m_pImgBuffer[index], 0, sizeof(BYTE) * m_nImageWidth[index] * m_nImageHeight[index] * 3);
        }
        else
        {
        m_pBmpInfo[index]->bmiHeader.biBitCount = 8;  // 彩色图像为24,黑白图像为8

            // 黑白相机需要进行初始化调色板操作
            for (int i = 0; i < 256; i++)
            {
                m_pBmpInfo[index]->bmiColors[i].rgbBlue = i;
                m_pBmpInfo[index]->bmiColors[i].rgbGreen = i;
                m_pBmpInfo[index]->bmiColors[i].rgbRed = i;
                m_pBmpInfo[index]->bmiColors[i].rgbReserved = i;
            }

            // 黑白图像Buffer分配
    m_pImgBuffer[index] = nullptr;
    m_pImgBuffer[index] = (BYTE*)malloc(sizeof(BYTE) * m_nImageWidth[index] * m_nImageHeight[index]);
    if (m_pImgBuffer[index] == nullptr)
    {
        bRet = false;
        LOG(INFO) << "error:" << m_nImageWidth[index] * m_nImageHeight[index] << std::endl;

        return bRet;
    }
    memset(m_pImgBuffer[index], 0, sizeof(BYTE) * m_nImageWidth[index] * m_nImageHeight[index]);
    }

    // 若分配失败则释放已分配的资源
    if (!bRet)
    {
    if (m_stFrameData[index].pImgBuf != nullptr)
            {
        free(m_stFrameData[index].pImgBuf);
        m_stFrameData[index].pImgBuf = nullptr;
            }

    if (m_pImgBuffer[index] != nullptr)
            {
        free(m_pImgBuffer[index]);
        m_pImgBuffer[index] = nullptr;
            }
    }

    return bRet;
}

int Camera_DH::getDeviceInitParam(int idx)
{
    GX_STATUS emStatus = GX_STATUS_ERROR;
    bool      bIsImplemented = false;

    // 查询当前相机是否支持GX_ENUM_PIXEL_COLOR_FILTER
    emStatus = GXIsImplemented(m_hDevice[idx], GX_ENUM_PIXEL_COLOR_FILTER, &bIsImplemented);
    VERIFY_STATUS(emStatus);
    m_bIsColorFilter[idx] = bIsImplemented;

    // 支持表示输出彩色图像
    if (bIsImplemented)
    {
        emStatus = GXGetEnum(m_hDevice[idx], GX_ENUM_PIXEL_COLOR_FILTER, &m_nPixelColorFilter[idx]);
        VERIFY_STATUS(emStatus);
    }

    // 获取宽度
    emStatus = GXGetInt(m_hDevice[idx], GX_INT_WIDTH, &m_nImageWidth[idx]);
    VERIFY_STATUS(emStatus);

    // 获取高度
    emStatus = GXGetInt(m_hDevice[idx], GX_INT_HEIGHT, &m_nImageHeight[idx]);
    VERIFY_STATUS(emStatus);

    // 获取图像大小
    emStatus = GXGetInt(m_hDevice[idx], GX_INT_PAYLOAD_SIZE, &m_nPayLoadSize[idx]);

    return emStatus;
}

int Camera_DH::initDevice(int idx, bool autoBalance, double exposure)
{
    //设置设备的默认参数，采集模式:连续采集,触发开关:开,触发源:软触发,数据格式:8-bit
    GX_STATUS emStatus = GX_STATUS_SUCCESS;

    //设置采集模式连续采集
    emStatus = GXSetEnum(m_hDevice[idx], GX_ENUM_ACQUISITION_MODE, GX_ACQ_MODE_CONTINUOUS);
    VERIFY_STATUS(emStatus);

    //设置触发模式为开
    emStatus = GXSetEnum(m_hDevice[idx], GX_ENUM_TRIGGER_MODE, GX_TRIGGER_MODE_ON);
    VERIFY_STATUS(emStatus);

    //选择触发源为软触发
    emStatus = GXSetEnum(m_hDevice[idx], GX_ENUM_TRIGGER_SOURCE, GX_TRIGGER_SOURCE_SOFTWARE);
    VERIFY_STATUS(emStatus);

    // 曝光 10W  白平衡自动
    emStatus = GXSetFloat(m_hDevice[idx], GX_FLOAT_EXPOSURE_TIME, exposure);
    VERIFY_STATUS(emStatus);

    if(autoBalance)
    {
        //设置自动白平衡光照环境，当前以自适应光源为使用环境
        emStatus = GXSetEnum(m_hDevice[idx], GX_ENUM_AWB_LAMP_HOUSE, GX_AWB_LAMP_HOUSE_ADAPTIVE);
        VERIFY_STATUS(emStatus);

        //设置连续自动白平衡
        emStatus = GXSetEnum(m_hDevice[idx], GX_ENUM_BALANCE_WHITE_AUTO, GX_BALANCE_WHITE_AUTO_CONTINUOUS);
        VERIFY_STATUS(emStatus);
    }

    return 0;
}

bool Camera_DH::openDevice(CameraDH_Link &left, CameraDH_Link &right)
{
    GX_STATUS emStatus = GX_STATUS_SUCCESS;
    bool      bRet = true;
    uint32_t  nDevNum = 0;

    // 枚举设备
    emStatus = GXUpdateDeviceList(&nDevNum, 1000);
    VERIFY_STATUS(emStatus);
    if(emStatus != GX_STATUS_SUCCESS)
    {
        LOG(ERROR) << "Find device failed!!\n";
        return false;
    }

    // 判断当前连接设备个数
    if (nDevNum <= 0)
    {
        LOG(ERROR) << "Not find device!!!" << std::endl;
        return false;
    }

    LOG(INFO) << "Device Num: " << nDevNum << std::endl;

    LOG(INFO) <<"left_camera: " << left.address.c_str()  << ", access: " << left.access << "\n";
    LOG(INFO) <<"right_camera: " << right.address.c_str() << ", access: " << right.access << "\n";

    //获取所有设备的基础信息
    LOG(INFO) << "Get All Device BaseInfo... " << std::endl;

    GX_DEVICE_BASE_INFO *pBaseinfo = new GX_DEVICE_BASE_INFO[nDevNum];
    size_t nSize = nDevNum * sizeof(GX_DEVICE_BASE_INFO);
    emStatus = GXGetAllDeviceBaseInfo(pBaseinfo, &nSize);
    VERIFY_STATUS(emStatus);

    /*
        GX_ACCESS_STATUS_UNKNOWN   = 0,

        GX_ACCESS_STATUS_READWRITE  = 1,

        GX_ACCESS_STATUS_READONLY   = 2,

        GX_ACCESS_STATUS_NOACCESS   = 3,
   */
    for(int i = 0 ; i < (int)nDevNum; i++)
    {
        m_accessStatus[i] = (int)pBaseinfo[i].accessStatus;
    }
    delete []pBaseinfo;

    LOG(INFO) <<"LeftCam Access: ";
    if(m_accessStatus[0] == 0)
        LOG(INFO) <<" UNKNOWN \n";
    else if(m_accessStatus[0] == 1)
        LOG(INFO) <<" READWRITE \n";
    else if(m_accessStatus[0] == 2)
        LOG(INFO) <<" READONLY \n";
    else if(m_accessStatus[0] == 3)
        LOG(INFO) <<" NOACCESS \n";

    LOG(INFO) <<"RightCam Access: ";
    if(m_accessStatus[1] == 0)
        LOG(INFO) <<" UNKNOWN \n";
    else if(m_accessStatus[1] == 1)
        LOG(INFO) <<" READWRITE \n";
    else if(m_accessStatus[1] == 2)
        LOG(INFO) <<" READONLY \n";
    else if(m_accessStatus[1] == 3)
        LOG(INFO) <<" NOACCESS \n";

    //打开相机
    emStatus = GX_STATUS_SUCCESS;

    GX_OPEN_PARAM  stOpenParam[2];

    for(int i = 0; i < 2; i++)
    {
        stOpenParam[i].openMode    = GX_OPEN_MAC;

        if(i == 0)
        {
            stOpenParam[i].accessMode  = left.access;
            stOpenParam[i].pszContent  = (char *)left.address.c_str();
        }
        else
        {
            stOpenParam[i].accessMode  = right.access;
            stOpenParam[i].pszContent  = (char *)right.address.c_str();
        }

        m_hDevice[i] = NULL;
        emStatus = GXOpenDevice(&stOpenParam[i], &m_hDevice[i]);
        VERIFY_STATUS(emStatus);

        if(emStatus != GX_STATUS_SUCCESS)
        {
            LOG(ERROR) << "Cam-" << i << " Open Failed!!\n";

            if(i == 0)
                continue;
            else
                return false;
        }

        if(i == 0)
        {
            // 打开自动白平衡，设置默认曝光时间为100000
            emStatus = initDevice(i, true, left.exposure);
            VERIFY_STATUS(emStatus);
        }
        else
        {
            emStatus = initDevice(i, true, right.exposure);
            VERIFY_STATUS(emStatus);
        }

        //获取设备的宽高等信息
        emStatus = getDeviceInitParam(i);
        VERIFY_STATUS(emStatus);

        bRet = prepareForImg(i);
        if (!bRet)
        {
            LOG(ERROR) << "malloc failed!!!" << std::endl;
            continue;
        }

        // 开始采集
        emStatus = GXSendCommand(m_hDevice[i], GX_COMMAND_ACQUISITION_START);
        VERIFY_STATUS(emStatus);
        if(emStatus != GX_STATUS_SUCCESS)
        {
            LOG(ERROR) << "Cam-" << i << " Start Accept Failed!!\n";

            if(i == 0)
                continue;
            else
                return false;
        }

        // 更新设备打开标识
        m_bIsOpen[i] = true;
        LOG(INFO) << "DH_dev-" << i << "  connect success!!\n";
    }

    return true;
}

void Camera_DH::closeDevice()
{
    GX_STATUS emStatus = GX_STATUS_ERROR;

    // 释放资源
    for(int i = 0 ; i < 2; i++)
    {
        if (m_stFrameData[i].pImgBuf != nullptr)
        {
            free(m_stFrameData[i].pImgBuf);
            m_stFrameData[i].pImgBuf = nullptr;
        }

        if (m_pImgBuffer[i] != nullptr)
        {
            free(m_pImgBuffer[i]);
            m_pImgBuffer[i] = nullptr;
        }

        // 关闭设备
        emStatus = GXCloseDevice(m_hDevice[i]);
        m_bIsOpen[i] = false;
    }

    return;
}

bool Camera_DH::is_connected(int idx)
{
    return m_bIsOpen[idx];
}

bool Camera_DH::capture_colorIMG(int idx, cv::Mat &img)
{
    if(!getImgBySoftTrigger(idx))
        return false;

    if (m_bIsColorFilter[idx])
    {
        cv::Mat mat;
        mat.create(cv::Size(m_nImageWidth[idx], m_nImageHeight[idx]), CV_8UC3);
        memcpy(mat.data, m_pImgBuffer[idx], m_nImageWidth[idx] * m_nImageHeight[idx] * 3);
        cv::flip(mat, mat, 0);
        img = mat.clone();
    }
    else
    {
        cv::Mat mat;
        mat.create(cv::Size(m_nImageWidth[idx], m_nImageHeight[idx]), CV_8UC1);
        memcpy(mat.data, m_pImgBuffer[idx], m_nImageWidth[idx] * m_nImageHeight[idx]);
        cv::flip(mat, mat, 0);
        img = mat.clone();
    }
    return true;
}


bool Camera_DH::getImgBySoftTrigger(int idx)
{
    if (!m_bIsOpen[idx])
    {
        return false;
    }

    GX_STATUS emStatus = GX_STATUS_ERROR;
    double dElapsedtime = 500;

    //每次发送触发命令之前清空采集输出队列
    //防止库内部缓存帧，造成本次GXGetImage得到的图像是上次发送触发得到的图
    emStatus = GXFlushQueue(m_hDevice[idx]);
    VERIFY_STATUS(emStatus);
    if(emStatus != GX_STATUS_SUCCESS)
    {
        LOG(ERROR) << "Cam-" << idx << " Flush Queue Failed!!\n";
        return false;
    }

    //发送软触发命令
    emStatus = GXSendCommand(m_hDevice[idx], GX_COMMAND_TRIGGER_SOFTWARE);
    VERIFY_STATUS(emStatus);
    if(emStatus != GX_STATUS_SUCCESS)
    {
        LOG(ERROR) << "Cam-" << idx << " Soft-Trigger Failed!!\n";
        return false;
    }

    //获取图像,默认超时500ms
    for(int i = 0 ; i < 5; i++)
    {
        emStatus = GXGetImage(m_hDevice[idx], &m_stFrameData[idx], dElapsedtime);
        if(emStatus != GX_STATUS_SUCCESS)
        {
            Sleep(20);
            continue;
        }
        else
            break;
    }

    if (m_stFrameData[idx].nStatus != 0)
    {
        LOG(ERROR) << "m_stFrameData IS NOT FULL!!! idx: " << idx << std::endl;
        return false;
    }

    //若支持彩色,转换为RGB图像后输出
    if (m_bIsColorFilter[idx])
    {
        //将Raw8图像转换为RGB图像以供显示
        DxRaw8toRGB24((BYTE*)m_stFrameData[idx].pImgBuf, m_pImgBuffer[idx], (VxUint32)m_nImageWidth[idx], (VxUint32)m_nImageHeight[idx], RAW2RGB_NEIGHBOUR,
            DX_PIXEL_COLOR_FILTER(m_nPixelColorFilter[idx]), TRUE);
    }
    else
    {
        // 黑白相机需要翻转数据后显示
        for (int i = 0; i < m_nImageHeight[idx]; i++)
        {
            memcpy(m_pImgBuffer[idx] + i * m_nImageWidth[idx], (BYTE*)(m_stFrameData[idx].pImgBuf) + (m_nImageHeight[idx] - i - 1) * m_nImageWidth[idx], (size_t)m_nImageWidth[idx]);
        }
    }
    return true;
}
